#include "encoder.h"

static EncoderInstance *encoder_instance;
EncoderInstance *EncoderRegister(EncoderConfig_s *config)
{
    EncoderInstance *instance = (EncoderInstance *)malloc(sizeof(EncoderInstance));
    instance->htim = config->htim;
    encoder_instance = instance;
    HAL_TIM_IC_Start_IT(instance->htim, TIM_CHANNEL_1);
    HAL_TIM_IC_Start_IT(instance->htim, TIM_CHANNEL_2);
    return instance;
}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (htim == encoder_instance->htim)
    {
        if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
        {
            encoder_instance->RiseCount = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
            encoder_instance->RiseCount = 1000;
            encoder_instance->last_duty = encoder_instance->duty;
            encoder_instance->duty = (float)encoder_instance->FallCount / encoder_instance->RiseCount;
            EncoderDecode(encoder_instance);
        }
        else if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2)
        {
            encoder_instance->FallCount = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
        }
    }
}

void EncoderDecode(EncoderInstance *instance)
{
    instance->angle_single_round = instance->duty * 360.0f;
    if (instance->duty - instance->last_duty > 0.5)
    {
        instance->total_round--;
    }
    else if (instance->duty - instance->last_duty < -0.5)
    {
        instance->total_round++;
    }
    instance->total_angle = instance->angle_single_round * instance->total_round;
}